/*
 * Copyright 2012 Alan Burlison, alan@bleaklow.com.  All rights reserved.
 * Use is subject to license terms.  See LICENSE.txt for details.
 */

#include "utils.h"
#include "TaskScheduler.h"

TaskScheduler::TaskScheduler(Task **_tasks, uint8_t _numTasks) :
  tasks(_tasks),
  numTasks(_numTasks) {
    // Initialise the tracing pins.
    SET_BIT_HI(TRACE_CAN_RUN_DDR, TRACE_CAN_RUN_PIN);
    SET_BIT_LO(TRACE_CAN_RUN_PORT, TRACE_CAN_RUN_PIN);
    SET_BIT_HI(TRACE_RUN_DDR, TRACE_RUN_PIN);
    SET_BIT_LO(TRACE_RUN_PORT, TRACE_RUN_PIN);
    SET_BIT_HI(TRACE_ISR_DDR, TRACE_ISR_PIN);
    SET_BIT_LO(TRACE_ISR_PORT, TRACE_ISR_PIN);
}

void TaskScheduler::run() {
    for (;;) {
        uint32_t now = millis();
        Task **tpp = tasks;
        for (uint16_t t = 0; t < numTasks; t++) {
            Task *tp = *tpp;
            SET_BIT_HI(TRACE_CAN_RUN_PORT, TRACE_CAN_RUN_PIN);
            if (tp->canRun(now)) {
                SET_BIT_LO(TRACE_CAN_RUN_PORT, TRACE_CAN_RUN_PIN);
                SET_BIT_HI(TRACE_RUN_PORT, TRACE_RUN_PIN);
                tp->run(now);
                SET_BIT_LO(TRACE_RUN_PORT, TRACE_RUN_PIN);
                break;
            } else {
                SET_BIT_LO(TRACE_CAN_RUN_PORT, TRACE_CAN_RUN_PIN);
            }
            tpp++;
        }
    }
}
